 /*
 *
 *  Copyright (c) 2003 ARM Limited
 *  All Rights Reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */
#include <linux/linkage.h>
#include <linux/init.h>
#include <mach/slave_core.h>


/*
 * Versatile Express specific entry point for secondary CPUs.  This
 * provides a "holding pen" into which all secondary cores are held
 * until we're ready for them to initialise.
 */

#ifdef CONFIG_RTOS_ONT_SMP_WITH_THUMB
.arm
#endif
.text
RTOS_CFI_TAG_ALIGN(5)
RTOS_ENTRY_NOTAG(hisi_reset_start)
#ifdef CONFIG_RTOS_HAL_SLAVECORE_BOOT_RETRY
/*
 * before enable mmu, if kernel trigger an exception, it's vector_table
 * will at 0x4 ~ 0x1c (VBAR = 0). before slave core booting, hisi_reset_start
 * code will copy into 0x0, so the first code of hisi_reset_start is the
 * vector table.
 * if slave core trigger exception before mmu, set it into wfi state
 * (in hisi_reset_start) and reboot it. (in hisi_boot_secondary)
 */
	b skip_vector
loop_wfi:
	wfi
	wfi
	wfi
	wfi
	wfi
	wfi
	wfi

	b loop_wfi
skip_vector:
#endif
	mov	r0, r0
	mov     r0, r0
	mov     r0, r0
	mov     r0, r0
	mov     r0, r0
	mov     r0, r0
	mov     r0, r0
	mov     r0, r0
#ifdef CONFIG_RTOS_HAL_CORE_RESET_LOG
	adr	r4, hisi_reset_start
	ldr	r3, =HISI_BOOT_LOG_ADDR_OFFSET
	add	r0, r4, r3
	ldr	r10, [r0]
	cmp	r10, 0
	beq	skip_reset_log

	mrc	p15, 0, r0, c0, c0, 5
	lsr	r3, r0, #8
	and	r3, r3, #0xf			@get cluster id
	and	r0, r0, #0xf			@get cpu id
	add	r0, r0, r3, lsl #2		@get true cpu id

	mov	r3, #0x61			@'a'
	add	r3, r3, r0
	strb	r3, [r10, r0]
skip_reset_log:
#endif
	/*
	 * set the cpu to SVC32 mode
	 */
	mrs	r0, cpsr
	bic	r0, r0, #0x1f
	orr	r0, r0, #0xd3		/*Disable irq&fiq, set svc model*/
	orr	r0, r0, #0x00000100	/*Disable async abort*/
	msr	cpsr,r0
	mov	r0, r0

#ifdef CONFIG_CPU_ENDIAN_BE8
	setend  be			/*Big endian, cpsr.e=1*/
#else
	setend  le			/*Little Endian*/
#endif
	mov	r0, r0

cache_set_hisi:
	/* ..MMU ICACHE DCACHE */
	movw	r0, #0x0878		/*disable ICache , Dcache , MMU*/
	movt	r0, #0x00c5
	mcr	p15, 0, r0, c1, c0, 0
	mov	r0, r0
	mov	r0, r0

	/* Invalidate caches/BTAC and TLB */
hisi_i_cache_invalid:
	mov     r10, #0
	mcr     p15, 0, r10, c7, c5, 0     @ invalidate I cache

	mrc     p15, 1, r0, c0, c0, 1      @read clidr
	ands    r3, r0, #0x7000000         @ extract loc from clidr
	mov     r3, r3, lsr #23            @ left align loc bit field
	beq     finished_hisi              @ if loc is 0, then no need to clean
	mov     r10, #0                    @ start clean at cache level 0 (in r10)
loop1_hisi:
	add     r2, r10, r10, lsr #1       @ work out 3x current cache level
	mov     r12, r0, lsr r2            @ extract cache type bits from clidr
	and     r12, r12, #7               @ mask of the bits for current cache only
	cmp     r12, #2                    @ see what cache we have at this level
	blt     skip_hisi                  @ skip if no cache, or just i-cache
	mcr     p15, 2, r10, c0, c0, 0     @ select current cache level in cssr
	mov     r12, #0
	mcr     p15, 0, r12, c7, c5, 4     @ prefetchflush to synch the new cssr&csidr
	mrc     p15, 1, r12, c0, c0, 0     @ read the new csidr
	and     r2, r12, #7                @ extract the length of the cache lines
	add     r2, r2, #4                @ add 4 (line length offset)
	ldr     r6, =0x3ff
	ands    r6, r6, r12, lsr #3        @ find maximum number on the way size
	clz     r5, r6                     @find bit position of way size increment
	ldr     r7, =0x7fff
	ands    r7, r7, r12, lsr #13       @ extract max number of the index size
loop2_hisi:
	mov     r8, r6                     @ create working copy of max way size
loop3_hisi:
	lsl     r4, r8, r5
	orr     r11, r10, r4               @ factor way and cache number into r11
	lsl     r4, r7, r2
	orr     r11, r11, r4               @ factor index number into r11
	mcr     p15, 0, r11, c7, c6, 2     @ invalidate by set/way
	subs    r8, r8, #1                 @ decrement the way
	bge     loop3_hisi
	subs    r7, r7, #1                 @decrement the index
	bge     loop2_hisi
skip_hisi:
	add     r10, r10, #2               @ increment cache number

#ifdef CONFIG_CORTEX_A15
        mrc     p15, 0, r4, c0, c0, 5
        and     r4, r4, #0xf
        cmp     r4, #0
        bne     finished_hisi              @ if not the first cpu in each cluster, do not need invalid l2 cache
#endif

	cmp     r3, r10
	bgt     loop1_hisi
finished_hisi:
	mov     r10, #0
	mcr     p15, 2, r10, c0, c0, 0     @ select current cache level in cssr, swith back to cache level 0
	mcr     p15, 0, r10, c7, c10, 4    @ drain write buffer

	mcr     p15, 0, r10, c8, c7, 0     @ invalidate I + D TLBs
	mcr     p15, 0, r10, c2, c0, 2     @ TTB control register
	dsb
	mov	r0, r0

	adr     r4, hisi_reset_start
	ldr     r3, =HISI_BOOT_TIMES_STORE
	add     r0, r4, r3
	ldr     r0, [r0]
	ldr	r1, =HISI_BOOT_MAGIC_NUM
	dsb
	cmp	r0, r1
	beq	boot_wfi_loop		   @ if need twice reset, goto wfi status after first reset


@/******************************************************/@


hisi_get_checkflag:
	ldr	r3, =HISI_BOOT_MAGIC_OFFSET1
	add	r0, r4, r3
	ldr	r0, [r0]
	dsb
	movw	r1, HISI_BOOT_MAGIC_NUM_L
	movt    r1, HISI_BOOT_MAGIC_NUM_H
	cmp	r0, r1
	bne	boot_from_point2

boot_from_point1:
	ldr	r3, =HISI_BOOT_ADDR_OFFSET1
	add	r0, r4, r3
	ldr	r1, [r0]		@read jump addr
	ldr     r2, [r0]		@read jump addr again
	dsb
	cmp	r1, r2
	bne	boot_from_point1

change_pc_point:
#ifdef CONFIG_CPU_ENDIAN_BE8
	rev	r1, r1
#endif
	mov     r0, r0
	mov	pc, r1

boot_from_point2:
	ldr     r3, =HISI_BOOT_MAGIC_OFFSET2
	add     r0, r4, r3
	ldr     r0, [r0]
	dsb
	cmp     r0, r1			@cmp with #0xa5a5a5a5
	bne     boot_wfi_loop
	ldr     r3, =HISI_BOOT_ADDR_OFFSET2
	add     r0, r4, r3		@boot_from_point2, the startaddress stored both in 0x8000f00 and 0x80000e00
	ldr     r1, [r0]          @read jump addr
	ldr     r2, [r0]          @read jump addr again
	dsb
	cmp     r1, r2
	bne     boot_from_point2
	b	change_pc_point

boot_wfi_loop:
	dsb
	wfi
	b	boot_wfi_loop

ENDPROC(hisi_reset_start)
	.ltorg    	@it is necessary for memcpy in core_reset.c
ENTRY(hisi_reset_start_end)
#ifdef CONFIG_RTOS_HKRR
	/*
	 * In the kernel link phase, the symbol address in the last symbol of
	 * the current file is the same as that in the next .o file. As a result,
	 * the HKrr relocation exception occurs. Therefore, the nop is added to
	 * prevent this situation.
	 */
	nop
#endif
#ifdef CONFIG_RTOS_ONT_SMP_WITH_THUMB
ENDPROC(hisi_reset_start_end)
#endif
